<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=utf-8">
<title>par_kinematics: /home/wouter/ros_packages/thp-pj2-08/par_kinematics/src/par_kinematics.cpp Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>

<table border="0">
<tr><td valign="top" style="border-right: 1px solid #333; padding-right: 10px;" >
<p>
Project: <a href="http://ros.org/wiki/par_kinematics">par_kinematics</a>
</p>
<p>
License: BSD
</p>
<p>
Dependencies: 
<ul>
  <li>std_msgs</li>
  <li>roscpp</li>
  <li>par_trajectory_planning</li>
</ul>
</p>
<p>
Used by:
None<br />
</p>
<p>
<a href="../../index.html">All Packages</a>
</p>

</div>
</td><td valign="top" style="padding-left: 10px;" >
<!-- Generated by Doxygen 1.7.1 -->
<div class="navigation" id="top">
  <div class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
    </ul>
  </div>
  <div class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<div class="header">
  <div class="headertitle">
<h1>/home/wouter/ros_packages/thp-pj2-08/par_kinematics/src/par_kinematics.cpp</h1>  </div>
</div>
<div class="contents">
<a href="par__kinematics_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00005"></a>00005 <span class="preprocessor">#include &lt;iostream&gt;</span>
<a name="l00006"></a>00006 <span class="preprocessor">#include &lt;ros/ros.h&gt;</span>
<a name="l00007"></a>00007 <span class="preprocessor">#include &lt;<a class="code" href="coord_8h.html">par_kinematics/coord.h</a>&gt;</span>
<a name="l00008"></a>00008 <span class="preprocessor">#include &lt;<a class="code" href="deltarobot_8h.html">par_kinematics/deltarobot.h</a>&gt;</span>
<a name="l00009"></a>00009 <span class="preprocessor">#include &lt;<a class="code" href="par__kinematics_8h.html">par_kinematics/par_kinematics.h</a>&gt;</span>
<a name="l00010"></a>00010 <span class="preprocessor">#include &lt;par_trajectory_planning/config.h&gt;</span>
<a name="l00011"></a>00011 
<a name="l00012"></a><a class="code" href="par__kinematics_8cpp.html#a9e4984bb4763bc6694e8a9b6a624f79d">00012</a> <span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">int</span> <a class="code" href="par__kinematics_8cpp.html#a9e4984bb4763bc6694e8a9b6a624f79d">QUEUE_SIZE</a> = 1000;
<a name="l00013"></a><a class="code" href="par__kinematics_8cpp.html#a7216565828409750bc51810d7c3db16c">00013</a> <span class="keyword">static</span> <a class="code" href="classDeltaRobot.html">DeltaRobot</a>* <a class="code" href="par__kinematics_8cpp.html#a7216565828409750bc51810d7c3db16c">deltaRobot</a>;
<a name="l00014"></a>00014 
<a name="l00019"></a><a class="code" href="par__kinematics_8cpp.html#aebf993f72e17cbaa18e6c8b83fb4fd20">00019</a> <span class="keywordtype">bool</span> <a class="code" href="par__kinematics_8cpp.html#aebf993f72e17cbaa18e6c8b83fb4fd20" title="This is the callback function responsible for publishing angles in radians.">IK_solver</a>(<a class="code" href="structpar__kinematics_1_1coordRequest__.html">par_kinematics::coord::Request</a>&amp; req,
<a name="l00020"></a>00020                <a class="code" href="structpar__kinematics_1_1coordResponse__.html">par_kinematics::coord::Response</a>&amp; res)
<a name="l00021"></a>00021 {
<a name="l00022"></a>00022     std::cout &lt;&lt; <span class="stringliteral">&quot;Inverse kinematics request received.&quot;</span> &lt;&lt; std::endl;
<a name="l00023"></a>00023     <span class="comment">// y and z are changed. this is how the model works. you could of course fix this</span>
<a name="l00024"></a>00024     <span class="comment">// by rewriting the code, but at the moment i don&#39;t have the time to do so.</span>
<a name="l00025"></a>00025         <span class="keywordtype">double</span> x = req.<a class="code" href="structpar__kinematics_1_1coordRequest__.html#ad9fedd97157b874e39ad3430d60c3ea8">x</a>;
<a name="l00026"></a>00026         <span class="keywordtype">double</span> y = req.<a class="code" href="structpar__kinematics_1_1coordRequest__.html#a54cc5c932e8324581f56cde28a76343a">z</a>;
<a name="l00027"></a>00027         <span class="keywordtype">double</span> z = req.<a class="code" href="structpar__kinematics_1_1coordRequest__.html#aa282de35c0456788768ff9a1b22b65da">y</a>;
<a name="l00028"></a>00028 
<a name="l00029"></a>00029     <a class="code" href="classPoint.html">Point</a> goal(x, y, z);
<a name="l00030"></a>00030         deltaRobot-&gt;<a class="code" href="classDeltaRobot.html#aed23f0594a258fad5176651242119fc6" title="Method for moving the delta robot to a specified point.">moveto</a>(goal);    
<a name="l00031"></a>00031         
<a name="l00032"></a>00032         res.<a class="code" href="structpar__kinematics_1_1coordResponse__.html#a165ac9c571766275923fd4627130204a">angles</a>[X] = <a class="code" href="classUtil.html#afdc08f7d944cb35d3360f5ae5478d8af" title="Function for converting radians to degrees.">Util::deg</a>(deltaRobot-&gt;<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[X]);
<a name="l00033"></a>00033         res.<a class="code" href="structpar__kinematics_1_1coordResponse__.html#a165ac9c571766275923fd4627130204a">angles</a>[Y] = <a class="code" href="classUtil.html#afdc08f7d944cb35d3360f5ae5478d8af" title="Function for converting radians to degrees.">Util::deg</a>(deltaRobot-&gt;<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[Y]);
<a name="l00034"></a>00034         res.<a class="code" href="structpar__kinematics_1_1coordResponse__.html#a165ac9c571766275923fd4627130204a">angles</a>[Z] = <a class="code" href="classUtil.html#afdc08f7d944cb35d3360f5ae5478d8af" title="Function for converting radians to degrees.">Util::deg</a>(deltaRobot-&gt;<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[Z]);        
<a name="l00035"></a>00035         
<a name="l00036"></a>00036         std::cout &lt;&lt; <span class="stringliteral">&quot;X: &quot;</span> &lt;&lt; <a class="code" href="classUtil.html#afdc08f7d944cb35d3360f5ae5478d8af" title="Function for converting radians to degrees.">Util::deg</a>(deltaRobot-&gt;<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[X]) &lt;&lt; std::endl;
<a name="l00037"></a>00037         std::cout &lt;&lt; <span class="stringliteral">&quot;Y: &quot;</span> &lt;&lt; <a class="code" href="classUtil.html#afdc08f7d944cb35d3360f5ae5478d8af" title="Function for converting radians to degrees.">Util::deg</a>(deltaRobot-&gt;<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[Y]) &lt;&lt; std::endl;
<a name="l00038"></a>00038         std::cout &lt;&lt; <span class="stringliteral">&quot;Z: &quot;</span> &lt;&lt; <a class="code" href="classUtil.html#afdc08f7d944cb35d3360f5ae5478d8af" title="Function for converting radians to degrees.">Util::deg</a>(deltaRobot-&gt;<a class="code" href="classDeltaRobot.html#abe45e9ddd8091b2b76a4d7ac49a22795">servo</a>[Z]) &lt;&lt; std::endl;
<a name="l00039"></a>00039 
<a name="l00040"></a>00040     <span class="keywordflow">return</span> <span class="keyword">true</span>;    
<a name="l00041"></a>00041 }
<a name="l00042"></a>00042 
<a name="l00043"></a><a class="code" href="par__kinematics_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627">00043</a> <span class="keywordtype">int</span> <a class="code" href="par__kinematics_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627">main</a>(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span> **argv)
<a name="l00044"></a>00044 {
<a name="l00045"></a>00045     std::cout &lt;&lt; <span class="stringliteral">&quot;par_kinematics_subscriber.&quot;</span> &lt;&lt; std::endl;
<a name="l00046"></a>00046     ros::init(argc, argv, <span class="stringliteral">&quot;par_kinematics_subscriber&quot;</span>);
<a name="l00047"></a>00047     ros::NodeHandle n;
<a name="l00048"></a>00048     deltaRobot = <span class="keyword">new</span> <a class="code" href="classDeltaRobot.html">DeltaRobot</a>(<a class="code" href="par__kinematics_8h.html#a7ca67cad9b3b6ef57950fe5bd738d17c">DELTA_ROBOT_HIP_LEN</a>, <a class="code" href="par__kinematics_8h.html#a3a64f2edb1e11d276fdb701105a24490">DELTA_ROBOT_ANK_LEN</a>, 
<a name="l00049"></a>00049                         <a class="code" href="par__kinematics_8h.html#a14d17acdcc41d79f62f221c68ba8a7ec">DELTA_ROBOT_BS_SZ</a>, <a class="code" href="par__kinematics_8h.html#ae2db878abe6aac0463a100671ee2c28c">DELTA_ROBOT_EF_SZ</a>);
<a name="l00050"></a>00050     
<a name="l00051"></a>00051     ros::ServiceServer service = n.advertiseService(<span class="stringliteral">&quot;coord&quot;</span>, <a class="code" href="par__kinematics_8cpp.html#aebf993f72e17cbaa18e6c8b83fb4fd20" title="This is the callback function responsible for publishing angles in radians.">IK_solver</a>);
<a name="l00052"></a>00052     
<a name="l00053"></a>00053     ros::spin();
<a name="l00054"></a>00054     
<a name="l00055"></a>00055     <span class="keywordflow">return</span> 0;
<a name="l00056"></a>00056 }
</pre></div></div>
</div>
</td></tr></table>

<hr size="1"><div style="align: right;">
<a href="http://ros.org/wiki/par_kinematics">par_kinematics</a><br />
Author(s): Wouter van Teijlingen</br />
<small>autogenerated on Mon Jun 13 2011 17:06:55</small>
</div>
</body>
</html>
